/*******************************************************************************
* File Name          : L3GD20.c
* Author             : Lancer3
* Version            : 1.0
* Date               : 5/13/2013
* Description        : L3GD20 driver file
*                      
* HISTORY:
* Date               |	Modification                    |	Author

*******************************************************************************/


/* Includes ------------------------------------------------------------------*/
#include "l3gd20.h"
#include "math.h"

/* Private typedef -----------------------------------------------------------*/

/* Private define ------------------------------------------------------------*/
#define L3GD20_SPI       SPI1
#define L3GD20_I2C       I2C1
#ifndef PI
#define PI				 3.14159265358979f
#endif
/* Private macro -------------------------------------------------------------*/

/* Private variables ---------------------------------------------------------*/
i16_t StdDev_X, StdDev_Y, StdDev_Z;

/* Private function prototypes -----------------------------------------------*/

/******************************************************************************
* Function Name		: L3GD20_ReadReg
* Description		: Reads one byte from register through I2C bus.		
* Input				: Register Address
* Input				: Variable to read data to
* Output			: Data Read
* Return			: MEMS_SUCCESS
*******************************************************************************
Master |ST|   | SAD + W |   | REG |     |SR|   | SAD + R |   |   |    |NMAK|SP|
Slave     |EV5|         |EV6|     | EV8 |  |EV5|         |EV6|EV7|DATA|    |  |
*******************************************************************************
*           Transmitter Mode                         Receiver Mode            * 
*     SAD + W = Slave address|0x00           SAD + R = Slave Address|0x01     *
******************************************************************************/
status_t L3GD20_ReadReg(u8_t Reg, u8_t* Data) {
  
	//Wait until I2C1 is not busy any more
	while(I2C_GetFlagStatus(I2C1, I2C_FLAG_BUSY));

	//Re-enable acknowledge
	I2C_AcknowledgeConfig(I2C1, ENABLE);

	//Send I2C1 START condition
	I2C_GenerateSTART(I2C1, ENABLE);
	//Wait for I2C1 EV5 --> Slave has acknowledged start condition
	while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_MODE_SELECT));

	//Send slave Address for write
	I2C_Send7bitAddress(I2C1, L3GD20_MEMS_I2C_ADDRESS, I2C_Direction_Transmitter);
	//Wait for I2C1 EV6, check if slave has acknowledged Master transmitter
	while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED));

	//Send address of register
	I2C_SendData(I2C1, Reg);
	//Wait for I2C1 EV8, check if slave has acknowledged Master Transmitter
	while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_TRANSMITTING));

	//Send START condition a second time (Re-Start)
	I2C_GenerateSTART(I2C1, ENABLE);
	//Wait for I2C1 EV5 --> Slave has acknowledged start condition
	while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_MODE_SELECT));

	//Send slave Address for read
	I2C_Send7bitAddress(I2C1, L3GD20_MEMS_I2C_ADDRESS, I2C_Direction_Receiver);

	//Send NMAK
	I2C_AcknowledgeConfig(I2C1, DISABLE);

	//Wait for I2C1 EV6, check if slave has acknowledged Master Receiver
	while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED));

	//Wait for I2C1 EV7 --> One byte has been received
	while( !I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_RECEIVED) );

	//Read data from I2C data register and return data byte
	*Data = I2C_ReceiveData(I2C1);

	I2C_GenerateSTOP(I2C1, ENABLE);
	//stop bit flag
	while(I2C_GetFlagStatus(I2C1, I2C_FLAG_STOPF));
	
  return MEMS_SUCCESS;
}

/******************************************************************************
* Function Name		: L3GD20_ReadBuffer
* Description		: Generic Reading function. It must be fulfilled with either
*					: I2C or SPI reading functions					
* Input				: Starting Register Address
* Output			: Data Buffer Read
* Return			: None
*******************************************************************************
Master|ST| |SAD + W| |REG|    |SR| |SAD + R|   |    |MAK|    |MAK|    |NMAK|SP|  
Slave   |EV5|     |EV6| |EV8_2|  |EV5|     |EV6|EV7DATA|   |DATA|   |DATA|
*******************************************************************************
*           Transmitter Mode                         Receiver Mode            * 
*     SAD + W = Slave address|0x00           SAD + R = Slave Address|0x01     *
******************************************************************************/
status_t L3GD20_ReadBuffer(uint8_t ReadAddr, AxesRaw_t* buff) {
  
	u8 XMSB,XLSB,YMSB,YLSB,ZMSB,ZLSB;

	//Test on BUSY Flag
	while (I2C_GetFlagStatus(I2C1,I2C_FLAG_BUSY));

	//Re-enable acknowledge
	I2C_AcknowledgeConfig(I2C1, ENABLE);

	//Send I2C1 START condition
	I2C_GenerateSTART(I2C1, ENABLE);
	//Wait for I2C1 EV5 --> Slave has acknowledged start condition
	while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_MODE_SELECT));
	
	//Send device address for write
	I2C_Send7bitAddress(I2C1, L3GD20_MEMS_I2C_ADDRESS, I2C_Direction_Transmitter);
	//Wait for I2C1 EV6, check if slave has acknowledged Master transmitter
	while (!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED));
	
	//Send the device's internal address to read from with MSB asserted
	I2C_SendData(I2C1,ReadAddr|0x80);
	//Wait for I2C1 EV8_2, check if slave has acknowledged Master Transmitter
	while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_TRANSMITTING));

	//Send START condition a second time (Re-Start)
	I2C_GenerateSTART(I2C1, ENABLE);
	//Wait for I2C1 EV5 --> Slave has acknowledged start condition
	while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_MODE_SELECT));
	
	//Send address for read
	I2C_Send7bitAddress(I2C1, L3GD20_MEMS_I2C_ADDRESS, I2C_Direction_Receiver);
	//Wait for I2C1 EV6, check if slave has acknowledged Master Receiver
	while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED));
	
	//Load in all 6 registers */
	while (!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_RECEIVED));
	XMSB = I2C_ReceiveData(I2C1);
	while (!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_RECEIVED));
	XLSB = I2C_ReceiveData(I2C1);
	while (!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_RECEIVED));
	YMSB = I2C_ReceiveData(I2C1);
	while (!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_RECEIVED));
	YLSB = I2C_ReceiveData(I2C1);
	while (!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_RECEIVED));
	ZMSB = I2C_ReceiveData(I2C1);
	//Disable ACK after last byte read and before actually reading it
	I2C_AcknowledgeConfig(I2C1, DISABLE);
	while (!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_RECEIVED));
	I2C_GenerateSTOP(I2C1, ENABLE);
	ZLSB = I2C_ReceiveData(I2C1);
	//Ensure stop bit was set.
	while(I2C_GetFlagStatus(I2C1, I2C_FLAG_STOPF));

	//Sort into 3 global variables
	buff->AXIS_X = (i16_t)((XMSB<<8) | XLSB);
	buff->AXIS_Y = (i16_t)((YMSB<<8) | YLSB);
	buff->AXIS_Z = (i16_t)((ZMSB<<8) | ZLSB);

	return MEMS_SUCCESS;
}

/*******************************************************************************
* Function Name		: L3GD20_WriteReg
* Description		: Generic write function.
* Input				: Register Address, Data to be written
* Output			: None
* Return			: None
*******************************************************************************
Master |ST|         |SAD + W|           |REG|           |DATA|             |SP|  
Slave        |EV5|              |EV6|          |EV8_2|            |EV5|    
*******************************************************************************
*           Transmitter Mode                         Receiver Mode            * 
*     SAD + W = Slave address|0x00           SAD + R = Slave Address|0x01     *
******************************************************************************/
status_t L3GD20_WriteReg(u8_t Reg, u8_t Data) {
    
	//I2C Write Sequence

	//Wait until I2C1 is not busy anymore
	while(I2C_GetFlagStatus(I2C1, I2C_FLAG_BUSY));

	// Send I2C1 START condition
	I2C_GenerateSTART(I2C1, ENABLE);
	//Wait for I2C1 EV5 --> Slave has acknowledged start condition
	while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_MODE_SELECT));

	//Send slave Address for write
	I2C_Send7bitAddress(I2C1, L3GD20_MEMS_I2C_ADDRESS, I2C_Direction_Transmitter);
	//Wait for I2C1 EV6, check if slave has acknowledged Master transmitter
	while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED));

	//Mode register address
	I2C_SendData(I2C1, Reg);
	//wait for byte send to complete
	while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_TRANSMITTED));
	
	//wait for byte send to complete
	while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_TRANSMITTED));
	//clear bits
	I2C_SendData(I2C1, Data);
	
	//generate stop
	I2C_GenerateSTOP(I2C1, ENABLE);
	//stop bit flag
	while(I2C_GetFlagStatus(I2C1, I2C_FLAG_STOPF));

	return MEMS_SUCCESS;
}

/* Private functions ---------------------------------------------------------*/

/*******************************************************************************
* Function Name  : L3GD20_Init
* Description    : Sets L3GD20 configuration
* Input          : None
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t L3GD20_Init(){

	L3GD20_SetODR(L3GD20_ODR_760Hz_BW_100);
	L3GD20_SetMode(L3GD20_NORMAL);
	L3GD20_SetAxis(L3GD20_X_ENABLE | L3GD20_Y_ENABLE | L3GD20_Z_ENABLE);
	L3GD20_SetFullScale(L3GD20_FULLSCALE_500);
	L3GD20_SetBDU(ENABLE);
	L3GD20_SetBLE(L3GD20_BLE_MSB);
	L3GD20_SetSPIInterface(L3GD20_SPI_3_WIRE);
	L3GD20_FIFOModeEnable(L3GD20_FIFO_DISABLE);
	L3GD20_SetOffset(L3GD20_OUT_X_L);

  return MEMS_SUCCESS;
}

/*******************************************************************************
* Function Name  : L3GD20_SetODR
* Description    : Sets L3GD20 Output Data Rate. ODR and BW are highest four
*                : bits of L3GD20_CTRL_REG1. Shift 'ov' left for bits, AND
*                : 'ov' with value read and write back to L3GD20_CTRL_REG1.
* Input          : Output Data Rate
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t L3GD20_SetODR(L3GD20_ODR_t ov){
  u8_t value;
  u8_t mask = 0x0F;

  if( !L3GD20_ReadReg(L3GD20_CTRL_REG1, &value) )
    return MEMS_ERROR;

  value = (value & mask) | (ov << 4);

  if( !L3GD20_WriteReg(L3GD20_CTRL_REG1, value) )
    return MEMS_ERROR;

  return MEMS_SUCCESS;
}


/*******************************************************************************
* Function Name  : L3GD20_SetMode
* Description    : Sets L3GD20 Operating Mode
* Input          : Modality (NORMAL, SLEEP, POWER_DOWN)
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t L3GD20_SetMode(L3GD20_Mode_t md) {
  u8_t value;
  u8_t mask = 0xF7;
  
  if( !L3GD20_ReadReg(L3GD20_CTRL_REG1, &value) )
    return MEMS_ERROR;
                  
  switch(md) {
  
  case L3GD20_POWER_DOWN:		
	value = (value & mask) | (MEMS_RESET<<L3GD20_PD);
    break;
          
  case L3GD20_NORMAL:
    value = (value & mask) | (MEMS_SET<<L3GD20_PD);
    break;
          
  case L3GD20_SLEEP:		
    mask = 0xF0;
    value = (value & mask) | ( (MEMS_SET<<L3GD20_PD) | (MEMS_RESET<<L3GD20_ZEN) | (MEMS_RESET<<L3GD20_YEN) | (MEMS_RESET<<L3GD20_XEN) );
    break;
          
  default:
    return MEMS_ERROR;
  }
  
  if( !L3GD20_WriteReg(L3GD20_CTRL_REG1, value) )
    return MEMS_ERROR;
                  
  return MEMS_SUCCESS;
}


/*******************************************************************************
* Function Name  : L3GD20_SetAxis
* Description    : Enable/Disable L3GD20 Axis
* Input          : X_ENABLE/X_DISABLE | Y_ENABLE/Y_DISABLE | Z_ENABLE/Z_DISABLE
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t L3GD20_SetAxis(L3GD20_Axis_t axis) {
  u8_t value;
  u8_t mask = 0xF8;
  
  if( !L3GD20_ReadReg(L3GD20_CTRL_REG1, &value) )
    return MEMS_ERROR;
    
  value = (value & mask) | axis;
  
  if( !L3GD20_WriteReg(L3GD20_CTRL_REG1, value) )
    return MEMS_ERROR;  

  return MEMS_SUCCESS;
}


/*******************************************************************************
* Function Name  : L3GD20_SetFullScale
* Description    : Sets the L3GD20 FullScale
* Input          : FS_250/FS_500/FS_2000
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t L3GD20_SetFullScale(L3GD20_Fullscale_t fs) {
  u8_t value;
  u8_t mask = 0xCF;
  
  if( !L3GD20_ReadReg(L3GD20_CTRL_REG4, &value) )
    return MEMS_ERROR;

  value = (value & mask) | (fs<<L3GD20_FS);
  
  if( !L3GD20_WriteReg(L3GD20_CTRL_REG4, value) )
    return MEMS_ERROR;

  return MEMS_SUCCESS;
}


/*******************************************************************************
* Function Name  : L3GD20_SetBDU
* Description    : Enable/Disable Block Data Update Functionality
* Input          : ENABLE/DISABLE
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t L3GD20_SetBDU(State_t bdu) {
  u8_t value;
  u8_t mask = 0x7F;
  
  if( !L3GD20_ReadReg(L3GD20_CTRL_REG4, &value) )
    return MEMS_ERROR;
 
  value = (value & mask) | (bdu<<L3GD20_BDU);

  if( !L3GD20_WriteReg(L3GD20_CTRL_REG4, value) )
    return MEMS_ERROR;

  return MEMS_SUCCESS;
}


/*******************************************************************************
* Function Name  : L3GD20_SetBLE
* Description    : Set Endianess (MSB/LSB)
* Input          : BLE_LSB / BLE_MSB
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t L3GD20_SetBLE(L3GD20_Endianess_t ble) {
  u8_t value;
  u8_t mask = 0xBF;
  
  if( !L3GD20_ReadReg(L3GD20_CTRL_REG4, &value) )
    return MEMS_ERROR;

  value = (value & mask) | (ble<<L3GD20_BLE);
  
  if( !L3GD20_WriteReg(L3GD20_CTRL_REG4, value) )
    return MEMS_ERROR;

  return MEMS_SUCCESS;
}

/*******************************************************************************
* Function Name  : L3GD20_SetOffset
* Description    : Set Endianess (MSB/LSB)
* Input          : BLE_LSB / BLE_MSB
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t L3GD20_SetOffset(u8_t Reg) {

	u8 XMSB = 0, XLSB = 0, YMSB = 0, YLSB = 0, ZMSB = 0, ZLSB = 0, i = 0;
	u8_t Xcnt = 0, Ycnt = 0, Zcnt = 0;
	i32_t X[100], Y[100], Z[100], temp;

	//Test on BUSY Flag
	while (I2C_GetFlagStatus(I2C1,I2C_FLAG_BUSY));

	//Re-enable acknowledge
	I2C_AcknowledgeConfig(I2C1, ENABLE);

	//Send I2C1 START condition
	I2C_GenerateSTART(I2C1, ENABLE);
	//Wait for I2C1 EV5 --> Slave has acknowledged start condition
	while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_MODE_SELECT));

	//Send device address for write
	I2C_Send7bitAddress(I2C1, L3GD20_MEMS_I2C_ADDRESS, I2C_Direction_Transmitter);
	//Wait for I2C1 EV6, check if slave has acknowledged Master transmitter
	while (!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED));

	//Send the device's internal address to read from with MSB asserted
	I2C_SendData(I2C1, Reg|0x80);
	//Wait for I2C1 EV8_2, check if slave has acknowledged Master Transmitter
	while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_TRANSMITTING));

	//Send START condition a second time (Re-Start)
	I2C_GenerateSTART(I2C1, ENABLE);
	//Wait for I2C1 EV5 --> Slave has acknowledged start condition
	while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_MODE_SELECT));

	//Send address for read
	I2C_Send7bitAddress(I2C1, L3GD20_MEMS_I2C_ADDRESS, I2C_Direction_Receiver);
	//Wait for I2C1 EV6, check if slave has acknowledged Master Receiver
	while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED));

	//For startup jitter
	for(i = 0; i < 10; i++)
	{
		while (!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_RECEIVED));
		XMSB = I2C_ReceiveData(I2C1);
		while (!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_RECEIVED));
		XLSB = I2C_ReceiveData(I2C1);
		while (!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_RECEIVED));
		YMSB = I2C_ReceiveData(I2C1);
		while (!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_RECEIVED));
		YLSB = I2C_ReceiveData(I2C1);
		while (!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_RECEIVED));
		ZMSB = I2C_ReceiveData(I2C1);
		while (!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_RECEIVED));
		ZLSB = I2C_ReceiveData(I2C1);
	}

	while (!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_RECEIVED));
	XMSB = I2C_ReceiveData(I2C1);
	while (!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_RECEIVED));
	XLSB = I2C_ReceiveData(I2C1);
	while (!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_RECEIVED));
	YMSB = I2C_ReceiveData(I2C1);
	while (!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_RECEIVED));
	YLSB = I2C_ReceiveData(I2C1);
	while (!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_RECEIVED));
	ZMSB = I2C_ReceiveData(I2C1);
	while (!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_RECEIVED));
	ZLSB = I2C_ReceiveData(I2C1);

	temp = (i16_t)((XMSB<<8) | XLSB);
	if(fabs(temp) < 100)
	{
		X[0] = temp;
		StdDev_X = X[0];
		Xcnt++;
	}
	else
		X[0] = 0;
	temp = (i16_t)((YMSB<<8) | YLSB);
	if(fabs(temp) < 100)
	{
		Y[0] = temp;
		StdDev_Y = Y[0];
		Ycnt++;
	}
	else
		Y[0] = 0;
	temp = (i16_t)((ZMSB<<8) | ZLSB);
	if(fabs(temp) < 100)
	{
		Z[0] = temp;
		StdDev_Z = Z[0];
		Zcnt++;
	}
	else
		Z[0] = 0;

	for (i = 1; i < 99; i++)
	{
		while (!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_RECEIVED));
		XMSB = I2C_ReceiveData(I2C1);
		while (!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_RECEIVED));
		XLSB = I2C_ReceiveData(I2C1);
		while (!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_RECEIVED));
		YMSB = I2C_ReceiveData(I2C1);
		while (!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_RECEIVED));
		YLSB = I2C_ReceiveData(I2C1);
		while (!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_RECEIVED));
		ZMSB = I2C_ReceiveData(I2C1);
		while (!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_RECEIVED));
		ZLSB = I2C_ReceiveData(I2C1);

		temp = (i16_t)((XMSB<<8) | XLSB);
		if(fabs(temp) < 100)
		{
			X[i] = temp;
			StdDev_X += X[i];
		}
		else
			X[i] = 0;
		temp = (i16_t)((YMSB<<8) | YLSB);
		if(fabs(temp) < 100)
		{
			Y[i] = temp;
			StdDev_Y += Y[i];
		}
		else
			Y[i] = 0;
		temp = (i16_t)((ZMSB<<8) | ZLSB);
		if(fabs(temp) < 100)
		{
			Z[i] = temp;
			StdDev_Z += Z[i];
		}
		else
			Z[i] = 0;
	}

	//Load in all 6 registers */
	while (!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_RECEIVED));
	XMSB = I2C_ReceiveData(I2C1);
	while (!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_RECEIVED));
	XLSB = I2C_ReceiveData(I2C1);
	while (!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_RECEIVED));
	YMSB = I2C_ReceiveData(I2C1);
	while (!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_RECEIVED));
	YLSB = I2C_ReceiveData(I2C1);
	while (!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_RECEIVED));
	ZMSB = I2C_ReceiveData(I2C1);
	//Disable ACK after last byte read and before actually reading it
	I2C_AcknowledgeConfig(I2C1, DISABLE);
	while (!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_RECEIVED));
	I2C_GenerateSTOP(I2C1, ENABLE);
	ZLSB = I2C_ReceiveData(I2C1);
	//Ensure stop bit was set.
	while(I2C_GetFlagStatus(I2C1, I2C_FLAG_STOPF));

	temp = (i16_t)((XMSB<<8) | XLSB);
	if(fabs(temp) < 100)
	{
		X[99] = temp;
		StdDev_X += X[99];
	}
	else
		X[99] = 0;
	temp = (i16_t)((YMSB<<8) | YLSB);
	if(fabs(temp) < 100)
	{
		Y[99] = temp;
		StdDev_Y += Y[99];
	}
	else
		Y[99] = 0;
	temp = (i16_t)((ZMSB<<8) | ZLSB);
	if(fabs(temp) < 100)
	{
		Z[99] = temp;
		StdDev_Z += Z[99];
	}
	else
		Z[99] = 0;


   //Caculate Variance
	//Calculate Mean - Simple average
	StdDev_X = StdDev_X / Xcnt;
	StdDev_Y = StdDev_Y / Ycnt;
	StdDev_Z = StdDev_Z / Zcnt;

	//For each number, subtract mean and square result
	StdDev_X = pow(X[0] - StdDev_X, 2);
	StdDev_Y = pow(Y[0] - StdDev_Y, 2);
	StdDev_Z = pow(Z[0] - StdDev_Z, 2);

	for(i = 1; i < 99; i++)
	{
		StdDev_X += pow(X[i] - StdDev_X, 2);
		StdDev_Y += pow(Y[i] - StdDev_Y, 2);
		StdDev_Z += pow(Z[i] - StdDev_Z, 2);
	}

	//Calculate Standard Mean of the results and multiply by 3 for offset
	StdDev_X = pow(StdDev_X / 100, 2) * 3;
	StdDev_Y = pow(StdDev_Y / 100, 2) * 3;
	StdDev_Z = pow(StdDev_Y / 100, 2) * 3;

	return MEMS_SUCCESS;

}


/*******************************************************************************
* Function Name  : L3GD20_HPFEnable
* Description    : Enable/Disable High Pass Filter
* Input          : ENABLE/DISABLE
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t L3GD20_HPFEnable(State_t hpf) {
  u8_t value;
  
  if( !L3GD20_ReadReg(L3GD20_CTRL_REG5, &value) )
    return MEMS_ERROR;
                  
  value &= 0xEF;
  value |= (hpf<<L3GD20_HPEN);
  
  if( !L3GD20_WriteReg(L3GD20_CTRL_REG5, value) )
    return MEMS_ERROR;
  
  return MEMS_SUCCESS;
}


/*******************************************************************************
* Function Name  : L3GD20_SetHPFMode
* Description    : Set High Pass Filter Modality
* Input          : HPM_NORMAL_MODE_RES/HPM_REF_SIGNAL/HPM_NORMAL_MODE/HPM_AUTORESET_INT
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t L3GD20_SetHPFMode(L3GD20_HPFMode_t hpf) {
  u8_t value;
  
  if( !L3GD20_ReadReg(L3GD20_CTRL_REG2, &value) )
    return MEMS_ERROR;
                  
  value &= 0xCF;
  value |= (hpf<<L3GD20_HPM);
  
  if( !L3GD20_WriteReg(L3GD20_CTRL_REG2, value) )
    return MEMS_ERROR;
  
  return MEMS_SUCCESS;
}


/*******************************************************************************
* Function Name  : L3GD20_SetHPFCutOFF
* Description    : Set High Pass CUT OFF Freq
* Input          : HPFCF_0,HPFCF_1,HPFCF_2... See Table 27 of the datasheet
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t L3GD20_SetHPFCutOFF(L3GD20_HPFCutOffFreq_t hpf) {
  u8_t value;
  
  if( !L3GD20_ReadReg(L3GD20_CTRL_REG2, &value) )
    return MEMS_ERROR;
                  
  value &= 0xF0;
  value |= (hpf<<L3GD20_HPFC0);
  
  if( !L3GD20_WriteReg(L3GD20_CTRL_REG2, value) )
    return MEMS_ERROR;
  
  return MEMS_SUCCESS;
  
}


/*******************************************************************************
* Function Name  : L3GD20_SetIntPinMode
* Description    : Set Interrupt Pin Modality (push pull or Open drain)
* Input          : PUSH_PULL/OPEN_DRAIN
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t L3GD20_SetIntPinMode(L3GD20_IntPinMode_t pm) {
  u8_t value;
  
  if( !L3GD20_ReadReg(L3GD20_CTRL_REG3, &value) )
    return MEMS_ERROR;
                  
  value &= 0xEF;
  value |= (pm<<L3GD20_PP_OD);
  
  if( !L3GD20_WriteReg(L3GD20_CTRL_REG3, value) )
    return MEMS_ERROR;
  
  return MEMS_SUCCESS;
}


/*******************************************************************************
* Function Name  : L3GD20_SetInt1Pin
* Description    : Set Interrupt1 pin Function
* Input          : L3GD20_I1_ON_PIN_INT1_ENABLE | L3GD20_I1_BOOT_ON_INT1 | L3GD20_INT1_ACTIVE_HIGH
* example        : L3GD20_SetInt1Pin(L3GD20_I1_ON_PIN_INT1_ENABLE | L3GD20_I1_BOOT_ON_INT1_ENABLE | L3GD20_INT1_ACTIVE_LOW) 
* to enable Interrupt 1 or Bootsatus on interrupt 1 pin
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t L3GD20_SetInt1Pin(L3GD20_Int1PinConf_t pinConf) {
  u8_t value;
  
  if( !L3GD20_ReadReg(L3GD20_CTRL_REG3, &value) )
    return MEMS_ERROR;
                  
  value &= 0x1F;
  value |= pinConf;
  
  if( !L3GD20_WriteReg(L3GD20_CTRL_REG3, value) )
    return MEMS_ERROR;
  
  return MEMS_SUCCESS;
}


/*******************************************************************************
* Function Name  : L3GD20_SetInt2Pin
* Description    : Set Interrupt2 pin Function
* Input          : L3GD20_I2_DRDY_ON_INT2_ENABLE/DISABLE | 
                   L3GD20_WTM_ON_INT2_ENABLE/DISABLE | 
                   L3GD20_OVERRUN_ON_INT2_ENABLE/DISABLE | 
                   L3GD20_EMPTY_ON_INT2_ENABLE/DISABLE
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t L3GD20_SetInt2Pin(L3GD20_Int2PinConf_t pinConf) {
  u8_t value;
  
  if( !L3GD20_ReadReg(L3GD20_CTRL_REG3, &value) )
    return MEMS_ERROR;
                  
  value &= 0xF0;
  value |= pinConf;
  
  if( !L3GD20_WriteReg(L3GD20_CTRL_REG3, value) )
    return MEMS_ERROR;
  
  return MEMS_SUCCESS;  
}


/*******************************************************************************
* Function Name  : L3GD20_Int1LatchEnable
* Description    : Enable Interrupt 1 Latching function
* Input          : ENABLE/DISABLE
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t L3GD20_Int1LatchEnable(State_t latch) {
  u8_t value;
  
  if( !L3GD20_ReadReg(L3GD20_INT1_CFG, &value) )
    return MEMS_ERROR;
                  
  value &= 0xBF;
  value |= latch<<L3GD20_LIR;
  
  if( !L3GD20_WriteReg(L3GD20_INT1_CFG, value) )
    return MEMS_ERROR;
  
  return MEMS_SUCCESS;
}


/*******************************************************************************
* Function Name  : L3GD20_ResetInt1Latch
* Description    : Reset Interrupt 1 Latching function
* Input          : None
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t L3GD20_ResetInt1Latch(void) {
  u8_t value;
  
  if( !L3GD20_ReadReg(L3GD20_INT1_SRC, &value) )
    return MEMS_ERROR;
  
  return MEMS_SUCCESS;
}


/*******************************************************************************
* Function Name  : L3GD20_SetIntConfiguration
* Description    : Interrupt 1 Configuration
* Input          : AND/OR, INT1_LIR ZHIE_ENABLE/DISABLE | INT1_ZLIE_ENABLE/DISABLE...
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t L3GD20_SetIntConfiguration(L3GD20_Int1Conf_t ic) {
  u8_t value;
  
  value = ic;

  if( !L3GD20_WriteReg(L3GD20_INT1_CFG, value) )
    return MEMS_ERROR;

  return MEMS_SUCCESS;
}


/*******************************************************************************
* Function Name  : L3GD20_SetInt1Threshold
* Description    : Sets Interrupt 1 Threshold
* Input          : Threshold = [0,31]
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t L3GD20_SetInt1Threshold(L3GD20_IntThsAxis axis, u16_t ths) {
  u8_t value;
  
  switch (axis) {
    
    case L3GD20_THS_X:
      //write the threshold LSB
      value = (u8_t)( ths & 0x00ff); 
      if( !L3GD20_WriteReg(L3GD20_INT1_THS_XL, value) )
        return MEMS_ERROR;
      
      //write the threshold LSB
      value = (u8_t)( ths >> 8); 
      if( !L3GD20_WriteReg(L3GD20_INT1_THS_XH, value) )
        return MEMS_ERROR;
      
      break;
      
    case L3GD20_THS_Y:
      //write the threshold LSB
      value = (u8_t)( ths & 0x00ff); 
      if( !L3GD20_WriteReg(L3GD20_INT1_THS_YL, value) )
        return MEMS_ERROR;
      
      //write the threshold LSB
      value = (u8_t)( ths >> 8); 
      if( !L3GD20_WriteReg(L3GD20_INT1_THS_YH, value) )
        return MEMS_ERROR;
      
      break;
      
    case L3GD20_THS_Z:
      //write the threshold LSB
      value = (u8_t)( ths & 0x00ff); 
      if( !L3GD20_WriteReg(L3GD20_INT1_THS_ZL, value) )
        return MEMS_ERROR;
      
      //write the threshold LSB
      value = (u8_t)( ths >> 8); 
      if( !L3GD20_WriteReg(L3GD20_INT1_THS_ZH, value) )
        return MEMS_ERROR;
      
      break;     
  }

  return MEMS_SUCCESS;
}


/*******************************************************************************
* Function Name  : L3GD20_SetInt1Duration
* Description    : Sets Interrupt 1 Duration
* Input          : Duration value
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t L3GD20_SetInt1Duration(L3GD20_Int1Conf_t id) {
 
  if (id > 127)
    return MEMS_ERROR;

  if( !L3GD20_WriteReg(L3GD20_INT1_DURATION, id) )
    return MEMS_ERROR;

  return MEMS_SUCCESS;
}


/*******************************************************************************
* Function Name  : L3GD20_FIFOModeEnable
* Description    : Sets Fifo Modality
* Input          : 
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t L3GD20_FIFOModeEnable(L3GD20_FifoMode_t fm) {
  u8_t value;
  u8_t mask = 0xBF;
  
  if(fm == L3GD20_FIFO_DISABLE) {
    
    if( !L3GD20_ReadReg(L3GD20_CTRL_REG5, &value) )
      return MEMS_ERROR;
                    
    value = (value & mask);
    
    if( !L3GD20_WriteReg(L3GD20_CTRL_REG5, value) )
      return MEMS_ERROR;
    
  }
  else {
    
    if( !L3GD20_ReadReg(L3GD20_CTRL_REG5, &value) )
      return MEMS_ERROR;
                    
    value = (value & mask) | MEMS_SET<<L3GD20_FIFO_EN;
    
    if( !L3GD20_WriteReg(L3GD20_CTRL_REG5, value) )
      return MEMS_ERROR;
    
    
    if( !L3GD20_ReadReg(L3GD20_FIFO_CTRL_REG, &value) )
      return MEMS_ERROR;
    
    value = (value & mask) | (fm<<L3GD20_FM0);
    
    if( !L3GD20_WriteReg(L3GD20_FIFO_CTRL_REG, value) )
      return MEMS_ERROR;
  }

  return MEMS_SUCCESS;
}


/*******************************************************************************
* Function Name  : L3GD20_SetWaterMark
* Description    : Sets Watermark Value
* Input          : Watermark = [0,31]
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t L3GD20_SetWaterMark(u8_t wtm) {
  u8_t value;
  
  if(wtm > 31)
    return MEMS_ERROR;  
  
  if( !L3GD20_ReadReg(L3GD20_FIFO_CTRL_REG, &value) )
    return MEMS_ERROR;
                  
  value &= 0xE0;
  value |= wtm; 
  
  if( !L3GD20_WriteReg(L3GD20_FIFO_CTRL_REG, value) )
    return MEMS_ERROR;
  
  return MEMS_SUCCESS;
}


/*******************************************************************************
* Function Name  : L3GD20_GetSatusReg
* Description    : Read the status register
* Input          : None
* Output         : status register buffer
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t L3GD20_GetSatusReg(u8_t* buff) {
  if( !L3GD20_ReadReg(L3GD20_STATUS_REG, buff) )
      return MEMS_ERROR;
  
  return MEMS_SUCCESS;  
}


/*******************************************************************************
* Function Name  : L3GD20_GetAngRateRaw
* Description    : Read the Angular Rate Registers
* Input          : None
* Output         : Angular Rate Registers buffer
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t L3GD20_GetAngRateRaw(AxesRaw_t* buff) {

	L3GD20_ReadBuffer(L3GD20_OUT_X_L, buff);

	/*u8_t valueL;
	u8_t valueH;

	if( !L3GD20_ReadReg(L3GD20_OUT_X_H, &valueH) )
		return MEMS_ERROR;

	if( !L3GD20_ReadReg(L3GD20_OUT_X_L, &valueL) )
		return MEMS_ERROR;

	buff->AXIS_X = (i16_t)( (valueH << 8) | valueL );

	if( !L3GD20_ReadReg(L3GD20_OUT_Y_H, &valueH) )
		return MEMS_ERROR;

	if( !L3GD20_ReadReg(L3GD20_OUT_Y_L, &valueL) )
		return MEMS_ERROR;

	buff->AXIS_Y = (i16_t)( (valueH << 8) | valueL );

	if( !L3GD20_ReadReg(L3GD20_OUT_Z_H, &valueH) )
		return MEMS_ERROR;

	if( !L3GD20_ReadReg(L3GD20_OUT_Z_L, &valueL) )
		return MEMS_ERROR;

	buff->AXIS_Z = (i16_t)( (valueH << 8) | valueL );*/

	return MEMS_SUCCESS;
}

/*******************************************************************************
* Function Name  : L3GD20_GetAngRateDeg
* Description    : Read the Angular Rate Registers and scale to degrees.
* Input          : Gyro pointer
* Output         : Angular Rate in degrees
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t L3GD20_GetAngRateDeg(AxesRaw_t* buff) {

	//int StdDev = 0;

	L3GD20_GetAngRateRaw(buff);

	if(fabsf(buff->AXIS_X - L3GD20_ZERO_RATE_500) < StdDev_X)
			buff->AXIS_X = 0;
	else
		buff->AXIS_X = L3GD20_SENSITIVITY_500 * (buff->AXIS_X - L3GD20_ZERO_RATE_500);

	if(fabsf(buff->AXIS_Y - L3GD20_ZERO_RATE_500) < StdDev_Y)
		buff->AXIS_Y = 0;
	else
		buff->AXIS_Y = L3GD20_SENSITIVITY_500 * (buff->AXIS_Y - L3GD20_ZERO_RATE_500);

	if(fabsf(buff->AXIS_Z - L3GD20_ZERO_RATE_500) < StdDev_Z)
		buff->AXIS_Z = 0;
	else
		buff->AXIS_Z = L3GD20_SENSITIVITY_500 * (buff->AXIS_Z - L3GD20_ZERO_RATE_500);

	return MEMS_SUCCESS;
}

/*******************************************************************************
* Function Name  : L3GD20_GetAngRateRad
* Description    : Read the Angular Rate Registers and scale to radians
* Input          : Gyro pointer
* Output         : Angular Rate in radians
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t L3GD20_GetAngRateRad(AxesRaw_t* buff) {

	//int StdDev = 0;
	
	L3GD20_GetAngRateRaw(buff);
	
	if(fabsf(buff->AXIS_X - L3GD20_ZERO_RATE_500) < StdDev_X)
		buff->AXIS_X = 0;
	else
		buff->AXIS_X = L3GD20_SENSITIVITY_500 * (buff->AXIS_X - L3GD20_ZERO_RATE_500);

	if(fabsf(buff->AXIS_Y - L3GD20_ZERO_RATE_500) < StdDev_Y)
		buff->AXIS_Y = 0;
	else
		buff->AXIS_Y = L3GD20_SENSITIVITY_500 * (buff->AXIS_Y - L3GD20_ZERO_RATE_500);

	if(fabsf(buff->AXIS_Z - L3GD20_ZERO_RATE_500) < StdDev_Z)
		buff->AXIS_Z = 0;
	else
		buff->AXIS_Z = L3GD20_SENSITIVITY_500 * (buff->AXIS_Z - L3GD20_ZERO_RATE_500);
		
	buff->AXIS_X = buff->AXIS_X * (PI/180);
	buff->AXIS_Y = buff->AXIS_Y * (PI/180);
	buff->AXIS_Z = buff->AXIS_Z * (PI/180);

	return MEMS_SUCCESS;  
}


/*******************************************************************************
* Function Name  : L3GD20_GetInt1Src
* Description    : Reset Interrupt 1 Latching function
* Input          : None
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t L3GD20_GetInt1Src(u8_t* buff) {
  
  if( !L3GD20_ReadReg(L3GD20_INT1_SRC, buff) )
    return MEMS_ERROR;
  
  return MEMS_SUCCESS;
}


/*******************************************************************************
* Function Name  : L3GD20_GetFifoSourceReg
* Description    : Read Fifo source Register
* Input          : None
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t L3GD20_GetFifoSourceReg(u8_t* buff) {
  
  if( !L3GD20_ReadReg(L3GD20_FIFO_SRC_REG, buff) )
    return MEMS_ERROR;
  
  return MEMS_SUCCESS;
}


/*******************************************************************************
* Function Name  : L3GD20_SetOutputDataAndFifoFilters
* Description    : ENABLE/DISABLE HIGH PASS and LOW PASS filters applied to output and fifo registers
*                : See Table 8 of AN3393 for more details
* Input          : L3GD20_NONE, L3GD20_HPH, L3GD20_LPF2, L3GD20_HPFLPF2
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t L3GD20_SetOutputDataAndFifoFilters(L3GD20_HPF_LPF2_Enable hpf){
  u8_t value;
  
  //HPF
  if( !L3GD20_ReadReg(L3GD20_CTRL_REG5, &value) )
    return MEMS_ERROR;
  
  switch(hpf) {
    
  case L3GD20_NONE:
    value &= 0xfc;
    value |= 0x00; //hpen = x, Out_sel_1 = 0, Out_sel_0 = 0
    break;
    
  case L3GD20_HPF:
    value &= 0xfc;
    value |= 0x01; //hpen = x, Out_sel_1 = 0, Out_sel_0 = 1
    break;

  case L3GD20_LPF2:
    value &= 0xed;
    value |= 0x02; //hpen = 0, Out_sel_1 = 1, Out_sel_0 = x
    break;    
   
  case L3GD20_HPFLPF2:
    value &= 0xed;
    value |= 0x12; //hpen = 1, Out_sel_1 = 1, Out_sel_0 = x
    break;    
  }
  
  if( !L3GD20_WriteReg(L3GD20_CTRL_REG5, value) )
    return MEMS_ERROR;
  
  return MEMS_SUCCESS;
  
}


/*******************************************************************************
* Function Name  : L3GD20_SetInt1Filters
* Description    : ENABLE/DISABLE HIGH PASS and LOW PASS filters applied to Int1 circuitery
*                : See Table 9 of AN3393 for more details
* Input          : NONE, HPH, LPF2, HPFLPF2
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t L3GD20_SetInt1Filters(L3GD20_HPF_LPF2_Enable hpf){
  u8_t value;
  
  //HPF
  if( !L3GD20_ReadReg(L3GD20_CTRL_REG5, &value) )
    return MEMS_ERROR;
  
  switch(hpf) {
    
  case L3GD20_NONE:
    value &= 0xf3;
    value |= 0x00<<L3GD20_INT1_SEL0; //hpen = x, Int1_sel_1 = 0, Int1_sel_0 = 0
    break;
    
  case L3GD20_HPF:
    value &= 0xf3;
    value |= 0x01<<L3GD20_INT1_SEL0; //hpen = x, Int1_sel_1 = 0, Int1_sel_0 = 1
    break;

  case L3GD20_LPF2:
    value &= 0xe7;
    value |= 0x02<<L3GD20_INT1_SEL0; //hpen = 0, Int1_sel_1 = 1, Int1_sel_0 = x
    break;    
   
  case L3GD20_HPFLPF2:
    value &= 0xe7;
    value |= 0x01<<L3GD20_HPEN;
    value |= 0x02<<L3GD20_INT1_SEL0; //hpen = 1, Int1_sel_1 = 1, Int1_sel_0 = x
    break;    
  }
  
  if( !L3GD20_WriteReg(L3GD20_CTRL_REG5, value) )
    return MEMS_ERROR;
  
  
  return MEMS_SUCCESS;
}


/*******************************************************************************
* Function Name  : L3GD20_SetSPIInterface
* Description    : Set SPI mode: 3 Wire Interface OR 4 Wire Interface
* Input          : L3GD20_SPI_3_WIRE, L3GD20_SPI_4_WIRE
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t L3GD20_SetSPIInterface(L3GD20_SPIMode_t spi) {
  u8_t value;
  u8_t mask = 0xFE;
  
  if( !L3GD20_ReadReg(L3GD20_CTRL_REG4, &value) )
    return MEMS_ERROR;
                  
  value = (value & mask) | spi<<L3GD20_SIM;
  
  if( !L3GD20_WriteReg(L3GD20_CTRL_REG4, value) )
    return MEMS_ERROR;
  
  return MEMS_SUCCESS;
}
